#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/io.h>
#include <pcl/filters/voxel_grid.h>

#include "pclview_tools.hpp"

// 单独测试，或许能加快编译速度
#include "opt_ICP_GTSAM.hpp"

struct AlignResult {
    double time;
    double score;
    Eigen::Matrix4f tf;
};

template <typename Registration>
AlignResult* test_pcl(Registration& reg, const CloudPtr& target, const CloudPtr& source) {
    CloudPtr aligned(new CloudType);
    AlignResult* Res = new AlignResult;

    // single run
    auto t1 = std::chrono::high_resolution_clock::now();
    reg.setInputTarget(target);
    reg.setInputSource(source);
    reg.align(*aligned);
    auto t2 = std::chrono::high_resolution_clock::now();
    Res->score = reg.getFitnessScore();
    Res->time = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6;
    Res->tf = reg.getFinalTransformation();
    std::cout << "time:" << Res->time << "[msec] score:" << Res->score << std::endl;

    return Res;
}

int main() {
    std::string config_file = "../config/config.yaml";
    // std::string config_file = "/home/qianliyue/rosdev/lio_learn_ws/src/testICP/config/config.yaml";
    YAML::Node config_node = YAML::LoadFile(config_file);
    std::string t_str = config_node["target_file"].as<std::string>();
    std::string s_str = config_node["source_file"].as<std::string>();
    std::cout << "load file: " << t_str << ", and " << s_str << std::endl;
    double ds_size = config_node["ds_size"].as<double>();

    CloudPtr cloud_source(new CloudType());
    CloudPtr cloud_target(new CloudType());
    if (pcl::io::loadPCDFile<PointType>(s_str, *cloud_source) == -1) {
        PCL_ERROR("Couldn't read file test_pcd.pcd\n");
        return (-1);
    }
    if (pcl::io::loadPCDFile<PointType>(t_str, *cloud_target) == -1) {
        PCL_ERROR("Couldn't read file test_pcd.pcd\n");
        return (-1);
    }

    pcl::VoxelGrid<PointType> ds_sample;
    ds_sample.setLeafSize(ds_size, ds_size, ds_size);
    ds_sample.setInputCloud(cloud_source);
    ds_sample.filter(*cloud_source);
    ds_sample.setInputCloud(cloud_target);
    ds_sample.filter(*cloud_target);


    std::cout << "--- ICP_GTSAM ---" << std::endl;
    CloudPtr transformed_source(new CloudType());
    TESTICP::opt_ICP_GTSAM icp_gtsam(config_node["ICP_GTSAM"]);
    AlignResult* Res = test_pcl(icp_gtsam, cloud_target, cloud_source);


    PclViewer<PointType> PclView(2, 1);
    PclView.addCloud(cloud_source,"cloud_source",1,{255,0,0});
    PclView.addCloud(cloud_target,"cloud_target",1,{255,255,255});

    PclView.addCloud(transformed_source,"transformed",2,{255,0,255});
    PclView.addCloud(cloud_target,"cloud_target",2,{255,255,255});

    PclView.show();
}
